// ManualBot.cpp : Defines the entry point for the console application.
//

#define _CRT_SECURE_NO_DEPRECATE 1
#include "stdafx.h"
#include "gnuplot_i.hpp"
#include "VectorField.h"
#include "AttractiveField.h"
#include "RepulseField.h"
#include "TangentField.h"
#include "RandomField.h"

void GraphTheFeilds();
void GraphTheAttractorFeild(VectorField& vf);
void GraphTheRepulsorFeild(std::vector<RepulseField>& vf);
void GraphTheTangentFeild(std::vector<TangentField>& vf);
void GraphTheCombinedFeild(VectorField& vf, std::vector<RepulseField>& repulses, std::vector<TangentField>& tangents);
void GraphTheRandomFeild();
void GraphWorld(Gnuplot& graph, bool obs, bool base);
void GraphTheAttractorVectors(VectorField&vf, Gnuplot& graph, double min, double max);
void GraphTheRepulsorVectors(std::vector<RepulseField>&vf, Gnuplot& graph, double min, double max);
void GraphTheTangentVectors(std::vector<TangentField>&vf, Gnuplot& graph, double min, double max);
void GraphTheCombinedVectors(VectorField& vf, std::vector<RepulseField>& repulses, std::vector<TangentField>& tangents, Gnuplot& graph, double min, double max);
void GraphTheRandomVectors(Gnuplot& graph, double min, double max);

#include <conio.h>   //for getch(), needed in wait_for_key()
#include <windows.h> //for Sleep()
void sleep(int i) { Sleep(i*1000); }
long GetTime();


int _tmain(int argc, char* argv[])
{
	string server;
	int port;

	if(argc < 2) {
		cout << "server: ";
		cin >> server;
	}
	else {
		server = argv[1];
	}
    if(argc < 3) {
		cout << "port: ";
		cin >> port;
	}
	else {
        port = atoi(argv[2]);
    }

	Remote::init(server, port, false);
	if (!Remote::instance()->GetStatus()) {
		cout << "Unable to contact server" << endl;
		exit(1);
	}

	World* world = World::instance();
	Team* team = Team::instance();

	while (true) {
		long t1 = GetTime();

		// First update our understanding of the world
		world->update();
		// Then have our team react to that knowledge
		team->update();

		long t2 = GetTime();
		cout << t2 - t1 << endl;
	}
	world->update();

	GraphTheFeilds();

	getchar();
	return 0;
}

long GetTime() {
	SYSTEMTIME tm;
	GetSystemTime(&tm);
	return (long)tm.wMilliseconds + 
		   (long)tm.wSecond * 1000 + 
		   (long)tm.wMinute * 60000 + 
		   (long)tm.wHour * 3600000;// + 
		   //(long)tm.wDay * 86400000;
}

void GraphTheFeilds(){
	World* world = World::instance();
	
	tank_t tank = world->tanks().at(0);
	flag_t flag = world->flags().at(2);
	flag_t base = world->flags().at(3);

	Vector2 flag_pos(flag.pos[0], flag.pos[1]);
	Vector2 base_pos(base.pos[0], base.pos[1]);
	Vector2 tank_loc(tank.pos[0], tank.pos[1]);

	vector <RepulseField> repulse_fields;
	vector <TangentField> tangential_fields;

	for (int i = 0; i < world->other_tanks().size(); i++) {
		otank_t* otank = &world->other_tanks().at(i);
		Vector2 otank_loc(otank->pos[0], otank->pos[1]);
		repulse_fields.push_back(RepulseField(otank_loc));
	}

	for(int i = 0; i < world->obstacles().size(); i ++){
		obstacle_t* obs = &world->obstacles().at(i);
		tangential_fields.push_back(TangentField(obs));
	}

	AttractiveField flag_field(flag_pos);
	AttractiveField base_field(base_pos);

	GraphTheAttractorFeild(flag_field);
	//GraphTheRepulsorFeild(repulse_fields);
	//GraphTheTangentFeild(tangential_fields);
	//GraphTheCombinedFeild(flag_field, repulse_fields, tangential_fields);
	//GraphTheRandomFeild();
}

void GraphTheAttractorFeild(VectorField& vf){
	Gnuplot graph;
	
	double min = -400,
		   max = 400;
	
	graph.set_title("Field");
	graph.set_grid();

	graph.set_xrange(min, max);
	graph.set_yrange(min, max);

	GraphWorld(graph, true, true);

	graph.set_lineStyle(6, 1, 1, "black");
	
	GraphTheAttractorVectors(vf, graph, min, max);

	graph << "set terminal png nocrop enhanced font verdana 12 size 1024,1024";
	graph << "set output \"Attract2.png\"";
	graph << "replot";

	getchar();
}

void GraphTheRepulsorFeild(std::vector<RepulseField>& vf){
	Gnuplot graph;
	
	double min = -400,
		   max = 400;
	
	graph.set_title("Field");
	graph.set_grid();

	graph.set_xrange(min, max);
	graph.set_yrange(min, max);

	graph.set_lineStyle(6, 1, 1, "black");

	GraphWorld(graph, true, true);
	
	GraphTheRepulsorVectors(vf, graph, min, max);

	graph << "set terminal png nocrop enhanced font verdana 12 size 1024,1024";
	graph << "set output \"Repulse2.png\"";
	graph << "replot";

	getchar();
}

void GraphTheTangentFeild(std::vector<TangentField>& vf){
	Gnuplot graph;
	double min = -300,
		   max = 300;
	
	graph.set_title("Field");
	graph.set_grid();

	graph.set_xrange(min, max);
	graph.set_yrange(min, max);

	GraphWorld(graph, true, false);
	
	graph.set_lineStyle(6, 1, 1, "black");

	GraphTheTangentVectors(vf, graph, min, max);

	graph << "set terminal png nocrop enhanced font verdana 12 size 2048,2048";
	graph << "set output \"Tangent2.png\"";
	graph << "replot";

	getchar();
}

void GraphTheCombinedFeild(VectorField& vf, std::vector<RepulseField>& repulses, std::vector<TangentField>& tangents){
	Gnuplot graph;
	double min = -400,
		   max = 400;
	
	graph.set_title("Field");
	graph.set_grid();

	graph.set_xrange(min, max);
	graph.set_yrange(min, max);

	GraphWorld(graph, true, false);
	
	graph.set_lineStyle(6, 1, 1, "black");

	GraphTheCombinedVectors(vf, repulses, tangents, graph, min, max);

	graph << "set terminal png nocrop enhanced font verdana 12 size 2048,2048";
	graph << "set output \"Combined2.png\"";
	graph << "replot";

	getchar();
}

void GraphTheRandomFeild(){
	Gnuplot graph;
	double min = -300,
		   max = 300;
	
	graph.set_title("Field");
	graph.set_grid();

	graph.set_xrange(min, max);
	graph.set_yrange(min, max);

	GraphWorld(graph, true, false);
	
	graph.set_lineStyle(6, 1, 1, "black");

	GraphTheRandomVectors(graph, min, max);

	graph << "set terminal png nocrop enhanced font verdana 12 size 2048,2048";
	graph << "set output \"Random.png\"";
	graph << "replot";

	getchar();
}

void GraphWorld(Gnuplot& graph, bool obs, bool base){
	World* world = World::instance();

	std::vector<double> x, y;

	bool plot = false;

	std::vector<obstacle_t> obstacles = world->obstacles();
	std::vector<team_t> teams = world->teams();


	graph.set_lineStyle(1, 1, 2, "black");
	
	if(obs)
	{
	graph.set_style("lines");
	for(unsigned int i = 0; i < obstacles.size(); i ++){
		x.clear(); y.clear();
		for(int ii = 0; ii < obstacles[i].size; ii ++){
			x.push_back(obstacles[i].o_corner[ii][0]);
			y.push_back(obstacles[i].o_corner[ii][1]);
		}
		x.push_back(obstacles[i].o_corner[0][0]);
		y.push_back(obstacles[i].o_corner[0][1]);

		graph.plot_xy(x, y, "", 1);
	}
	}
	if(base){
	for(unsigned int i = 0; i < teams.size(); i ++){
		x.clear(); y.clear();
		for(int ii = 0; ii < 4; ii ++){
			x.push_back(teams[i].base_corner[ii][0]);
			y.push_back(teams[i].base_corner[ii][1]);
		}
		x.push_back(teams[i].base_corner[0][0]);
		y.push_back(teams[i].base_corner[0][1]);

		graph.set_lineStyle(i+2, 1, 2, teams[i].color);

		graph.plot_xy(x, y, "", i+2);
	}
	}
}

void GraphTheAttractorVectors(VectorField&vf, Gnuplot& graph, double min, double max){
	std::vector<double> x, y, rx, ry;
	int step = 10;
	double scale;
	Vector2 direction;

	graph.set_style("vectors");
	for(int i = min; i <= (int)max; i+=step){
		for(int j = min; j <= (int)max; j+=step){
			x.push_back(i); y.push_back(j);
			direction = vf.get_trajectory(Vector2(i,j));
			if(direction.magnitude() > 40){
				scale = 1;
			}else{
				scale = direction.magnitude()/40;
			}
			direction = direction.normalize() * scale * 8.0;
			rx.push_back(direction.x);
			ry.push_back(direction.y);
		}
	}

	graph.plot_vectors(x, y, rx, ry);
}

void GraphTheRepulsorVectors(std::vector<RepulseField>&vf, Gnuplot& graph, double min, double max){
	std::vector<double> x, y, rx, ry;
	int step = 15;
	double scale;
	Vector2 direction;

	graph.set_style("vectors");
	for(int i = min; i <= (int)max; i+=step){
		for(int j = min; j <= (int)max; j+=step){
			direction.x = 0; direction.y = 0;
			for(int k = 0; k <  vf.size(); k++)
				direction += vf[k].get_trajectory(Vector2(i,j));

			if(direction.magnitude() != 0){
				x.push_back(i); y.push_back(j);
			
				direction = direction * 10.0;
				rx.push_back(direction.x);
				ry.push_back(direction.y);
			}
		}
	}

	graph.plot_vectors(x, y, rx, ry);
}

void GraphTheTangentVectors(std::vector<TangentField>&vf, Gnuplot& graph, double min, double max){
	std::vector<double> x, y, rx, ry;
	int step = 10;
	double scale;
	Vector2 direction;

	graph.set_style("vectors");
	for(int i = min; i <= (int)max; i+=step){
		for(int j = min; j <= (int)max; j+=step){
			direction.x = 0; direction.y = 0;
			for(int k = 0; k <  vf.size(); k++)
				direction += vf[k].get_trajectory(Vector2(i,j));

			if(direction.magnitude() != 0){
				x.push_back(i); y.push_back(j);
			
				direction = direction * 8.0;
				rx.push_back(direction.x);
				ry.push_back(direction.y);
			}
		}
	}

	graph.plot_vectors(x, y, rx, ry);
}

void GraphTheCombinedVectors(VectorField& vf, std::vector<RepulseField>& repulses, std::vector<TangentField>& tangents, Gnuplot& graph, double min, double max){
	std::vector<double> x, y, rx, ry;
	int step = 20;
	double scale;
	double tmag = 40;
	Vector2 direction;

	graph.set_style("vectors");
	for(int i = min; i <= (int)max; i+=step){
		for(int j = min; j <= (int)max; j+=step){
			direction.x = 0; direction.y = 0;
			
			Vector2 tank_loc(i,j);
			
			Vector2 attract_trajectory = vf.get_trajectory(tank_loc);
			
			attract_trajectory = attract_trajectory.normalize();

			Vector2 repulse_trajectory;

			for (int i = 0; i < repulses.size(); i++) {
				repulse_trajectory += repulses.at(i).get_trajectory(tank_loc);
			}

			double repulse_scale = 1.0f;
			if (repulse_trajectory.magnitude() > 0)
				repulse_trajectory = repulse_trajectory.normalize() * repulse_scale;

			Vector2 tangential_trajectory;

			for (int i = 0; i < tangents.size(); i++) {
				tangential_trajectory += tangents.at(i).get_trajectory(tank_loc);
			}

			double tangential_scale = 2.0f;
			if(tangential_trajectory.magnitude() != 0)  
				tangential_trajectory = tangential_trajectory.normalize() * tangential_scale;

			RandomField random_field;
			Vector2 random_trajectory = random_field.get_trajectory(tank_loc);

			Vector2 trajectory = attract_trajectory + repulse_trajectory + tangential_trajectory + random_trajectory;

			x.push_back(i); y.push_back(j);
			
			direction = trajectory * 10.0;
			rx.push_back(direction.x);
			ry.push_back(direction.y);
		}
	}

	graph.plot_vectors(x, y, rx, ry);
}

void GraphTheRandomVectors(Gnuplot& graph, double min, double max){
	std::vector<double> x, y, rx, ry;
	int step = 10;
	double scale;
	Vector2 direction;

	RandomField random_field;

	graph.set_style("vectors");
	for(int i = min; i <= (int)max; i+=step){
		for(int j = min; j <= (int)max; j+=step){
			direction.x = 0; direction.y = 0;
			
			direction = random_field.get_trajectory(Vector2(i, j));

			if(direction.magnitude() != 0){
				direction = direction.normalize();
				x.push_back(i); y.push_back(j);
			
				direction = direction * 8.0;
				rx.push_back(direction.x);
				ry.push_back(direction.y);
			}
		}
	}

	graph.plot_vectors(x, y, rx, ry);
}